#include <gtest/gtest.h>
#include <limits>
#include "Vtl/Vtl"

using namespace Vtl;
using namespace Eigen;

TEST(RadToDegTest,FullTest)
{
	EXPECT_DOUBLE_EQ(0.0,angles::rad2deg(0.0));
	EXPECT_DOUBLE_EQ(90.0,angles::rad2deg(M_PI*0.5));
	EXPECT_DOUBLE_EQ(180.0,angles::rad2deg(M_PI));
	EXPECT_DOUBLE_EQ(270,angles::rad2deg(M_PI*1.5));
	EXPECT_DOUBLE_EQ(360,angles::rad2deg(M_PI*2));
}
TEST(DegToRad,FullTest)
{
	EXPECT_DOUBLE_EQ(0.0,angles::deg2rad(0.0));
	EXPECT_DOUBLE_EQ(M_PI*0.5,angles::deg2rad(90.0));
	EXPECT_DOUBLE_EQ(M_PI,angles::deg2rad(180.0));
	EXPECT_DOUBLE_EQ(M_PI*1.5,angles::deg2rad(270.0));
	EXPECT_DOUBLE_EQ(M_PI*2,angles::deg2rad(360.0));
}

TEST(NormAngleTest,PositiveTest)
{
	EXPECT_DOUBLE_EQ(0.0, angles::normAngle(0.0));
	EXPECT_DOUBLE_EQ(M_PI/2.0, angles::normAngle(M_PI/2.0));
	EXPECT_DOUBLE_EQ(-M_PI, angles::normAngle(M_PI));
	EXPECT_DOUBLE_EQ(M_PI/2.0, angles::normAngle(M_PI*2.5));
	EXPECT_DOUBLE_EQ(-M_PI, angles::normAngle(M_PI*3));
	EXPECT_DOUBLE_EQ(-M_PI/2.0, angles::normAngle(M_PI*3.5));
	EXPECT_DOUBLE_EQ(0, angles::normAngle(M_PI*4.0));
}

TEST(NormAngleTest,NegativeTest)
{
	EXPECT_DOUBLE_EQ(0.0, angles::normAngle(0.0));
	EXPECT_DOUBLE_EQ(-M_PI/2.0, angles::normAngle(-M_PI/2.0));
	EXPECT_DOUBLE_EQ(M_PI, angles::normAngle(-M_PI));
	EXPECT_DOUBLE_EQ(-M_PI/2.0, angles::normAngle(-M_PI*2.5));
	EXPECT_DOUBLE_EQ(M_PI, angles::normAngle(-M_PI*3));
	EXPECT_DOUBLE_EQ(M_PI/2.0, angles::normAngle(-M_PI*3.5));
	EXPECT_DOUBLE_EQ(0, angles::normAngle(-M_PI*4.0));
}

TEST(GetAngleTest, FullTest)
{
	EXPECT_DOUBLE_EQ(0, angles::getAngle(Vector3d(0,1,0),Vector3d(0,1,0)));
	EXPECT_DOUBLE_EQ(M_PI/2, angles::getAngle(Vector3d(0,1,0),Vector3d(1,0,0)));
	EXPECT_DOUBLE_EQ(M_PI, angles::getAngle(Vector3d(0,1,0),Vector3d(0,-1,0)));
	EXPECT_DOUBLE_EQ(M_PI/2, angles::getAngle(Vector3d(0,1,0),Vector3d(-1,0,0)));
}

TEST(GetDirectionTest, FullTest)
{
	EXPECT_DOUBLE_EQ(0, angles::getDirection(Vector3d(0,1,1)));
	EXPECT_DOUBLE_EQ(M_PI/4, angles::getDirection(Vector3d(1,1,1)));
	EXPECT_DOUBLE_EQ(M_PI/2, angles::getDirection(Vector3d(1,0,1)));
	EXPECT_DOUBLE_EQ(-M_PI/2, angles::getDirection(Vector3d(-1,0,1)));
	EXPECT_DOUBLE_EQ(M_PI, angles::getDirection(Vector3d(0,-1,1)));
}

TEST(GetElevationTest, FullTest)
{
	EXPECT_DOUBLE_EQ(0, angles::getElevation(Vector3d(0,1,0)));
	EXPECT_DOUBLE_EQ(M_PI/4, angles::getElevation(Vector3d(0,1,1)));
	EXPECT_DOUBLE_EQ(M_PI/2, angles::getElevation(Vector3d(0,0,1)));
	EXPECT_DOUBLE_EQ(-M_PI/2, angles::getElevation(Vector3d(0,0,-1)));
	EXPECT_DOUBLE_EQ(-M_PI/4, angles::getElevation(Vector3d(0,-1,-1)));
}